function [ PNorm ] = normmat_initcon( P, P0 )
%NORMMAT_INITCON 使用初始值归一化矩阵
%
% Input Arguments:
% P: m*m*p矩阵，p为采样数
% P0: P阵的初始值，若为空，则设为P的第一个采样值，默认为空
%
% Output Arguments:
% PNorm: m*m*p矩阵，用初始值归一化后的矩阵

if nargin < 2
    P0 = P(:, :, 1);
end

P0DiagSqrtInv = (1./sqrt(diag(P0)));
PNorm = NaN(size(P));
for i = 1:size(P, 3)
    PNorm(:, :, i) = diag(P0DiagSqrtInv) * P(:, :, i) * diag(P0DiagSqrtInv);
end
end